#include "TargetFeeder.hpp"
#include "MatWork.hpp"



void TargetFeeder::on_mouse( int event, int x, int y, int flags, void* param )
{
	//	if( !frame )
	//		return;

	uchar ch1,ch2, ch3;	
    //Point3i sample; 	  // why can't I init Point3i
	int a,b,c;


	switch( event )
	{
		case CV_EVENT_LBUTTONDOWN:

	//		sample.x = ch1;
	//		sample.y = ch2;
	//		sample.z = ch3;


			printf("click point is (%d,%d) -> (R,G,B) = (%d,%d,%d)\n",x,y);

			clicked_points.push_back(Point2i(x, y));

			//writeTextFile();

			break;

		case CV_EVENT_LBUTTONUP:
			break;
	}
}


TargetFeeder::TargetFeeder(Mat& InputImage, Mat& InputObstacles)
{
	// get map - array of points & an image
	InputImage.copyTo(map_image);
	InputObstacles.copyTo(map_obstacles);
}


// visualize the map and use mouse click to define the workspace
void TargetFeeder::visualize()
{
	Point2i p0, p1;
	//int i;
	
	//printf("trajectory");
	//MatInfo(trajectory);
	//printf("trajectory data");
	//MATmtxdisp(trajectory);

	for (int i=0; i<trajectory.cols-1; i++)
	//for (i=0; i<via_points.size()-1; i++)
	{
		p0.x = cvRound(trajectory.at<Vec2f>(0, i)[0]);
		p0.y = cvRound(trajectory.at<Vec2f>(0, i)[1]);
		p1.x = cvRound(trajectory.at<Vec2f>(0, i+1)[0]); 		
		p1.y = cvRound(trajectory.at<Vec2f>(0, i+1)[1]);

	//	p0.x = cvRound(via_points.at(i).x);
	//	p0.y = cvRound(via_points.at(i).y);
	//	p1.x = cvRound(via_points.at(i+1).x); 		
	//	p1.y = cvRound(via_points.at(i+1).y);

		//printf("(%0.2f, %0.2f)->(%0.2f, %0.2f)|",p0.x, p0.y, p1.x, p1.y);
		printf("abcd(%d, %d)->(%d, %d)|",p0.x, p0.y, p1.x, p1.y);
		//printf("Hello !!");
		line(map_image, p0, p1, CV_RGB(255, 0, 0), 2, 8);

	//	trajectory.at<Vec2f>(0, i)[0] = p0.x;
	//	trajectory.at<Vec2f>(0, i)[1] = p0.y;
	}

	//trajectory.at<Vec2f>(i+1)[0] = p1.x;
	//trajectory.at<Vec2f>(i+1)[1] = p1.y;

//	printf("trajectory");
//	MatInfo(trajectory);
//	getchar();
	drawCircles(map_image, trajectory, CV_RGB(255, 0, 0));


	//line(map_image, Point2i(0, 0) , Point2i(100, 100), CV_RGB(255, 0, 0), 2, 8);
	//line(map_image, Point2i(100, 100) , Point2i(100, 300), CV_RGB(255, 0, 0), 2, 8);
	//line(map_image, Point2i(100, 300), p1, CV_RGB(255, 0, 0), 2, 8);

	imshow("Expected Trajectory", map_image);
	//waitKey(0);
}
/*
void TargetFeeder::visualize()
{
	char* wind_name = "Choose Workspace";
	char c;

	// draw obstacles on image
	drawCircles(map_image, map_obstacles);
	
	imshow(wind_name, map_image);

	//setMouseCallback(wind_name, on_mouse, 0);   // bug is here! cannot use mouseCallback

	// click 2 corners of workspace
	while (clicked_points.size() < 2)
	{
		printf("Please click 2 POINTS to create workspace!!");

		c = cvWaitKey(1);
		if (c == 27) 
		{
			break;
		}
	}
};
*/

// generate random targets with normal distribution
void TargetFeeder::generate(int num_point)
{
	trajectory = Mat(1, num_point, CV_32FC2, 0);

	printf("via points here\n");
	//MatInfo(trajectory);

	Mat via_pnts_col = Mat::zeros(1, num_point, CV_32F);
	Mat via_pnts_row = Mat::zeros(1, num_point, CV_32F);

	rng.fill(via_pnts_col, RNG::UNIFORM, clicked_points[0].x, clicked_points[1].x);  // ham RNG co' van de, bo so random number la nhu nhau o nhieu lan khoi tao khac nhau -> I used it wrong!!!
	rng.fill(via_pnts_row, RNG::UNIFORM, clicked_points[0].y, clicked_points[1].y);

	Point2f p;
	for (int i=0; i<num_point; i++)
	{
		p.x = via_pnts_col.at<float>(i);  // still have to use float, even cannot use double
		p.y = via_pnts_row.at<float>(i);
		

		printf("(%0.2f, %0.2f)",p.x, p.y);

		via_points.push_back(p);
	}

	// feed to return trajectory
	Mat tmp= Mat(via_points, 1);  // cannot be used -> used OK -> but is changed to column vector by default

	trajectory = tmp.reshape(2, 1);

	//printf("trajectory here:::");
	//MATmtxdisp(trajectory);
};


// create path through all points - Dijsktra or not :))

TargetFeeder::~TargetFeeder(void)
{

}











